#include "SolveEqu.h"
//采用LLT求解线性方程组,计算方程歧义,也能求解
namespace Solver
{

SolveEqu::SolveEqu()
{

}

SolveEqu::~SolveEqu()
{

}

void SolveEqu::solveLineEqu(const MatrixXd_t &vA, const VectorXd_t &b, VectorXd_t &x, const value_t r)
{
    solveLineEquForSymMatrix(vA.transpose() * vA, vA.transpose() * b, x, r);
}

void SolveEqu::solveLineEquForSymMatrix(const MatrixXd_t &vA, const VectorXd_t &b, VectorXd_t &x, const value_t r)
{
    //1.
    if (vA.rows() != vA.cols())
    {
        solveLineEqu(vA, b, x, r);
        return;
    }

    //2. 分解
    decompositionLLT(vA, r);

    //3. 求解
    solveEquByLLT(b, x);
}

value_t SolveEqu::decompositionLLT(const MatrixXd_t &vA, value_t damp)
{
    //1.
    const std::size_t n = vA.rows();
    if ((std::size_t)vL_.rows() != n)
    {
        vL_.resize(n, n);
    }

    //2.
    value_t sumii, sumij;
stepStart:
	for (std::size_t i = 0; i != n; ++i)
	{
		sumii = 0.0;
		for (std::size_t j = 0; j != i; ++j)
		{
			sumij = 0.0;
			for (std::size_t k = 0; k != j; ++k)
			{
				sumij += vL_(i,k) * vL_(j,k) * vL_(k,k);
			}

			sumij = (vA(i,j) - sumij) / vL_(j,j);
			sumii += vL_(j,j) * std::pow(sumij, 2);
			vL_(i,j) = sumij;
		}

		sumii = vA(i,i) - sumii + damp;
		if (std::fabs(sumii) < 1e-8)
		{
			if (std::fabs(damp) < 0.0001)
			{
				damp = 0.001;
			}
			else
			{
				damp += damp;
			}

			goto stepStart;
		}

		vL_(i,i) = sumii;
	}

	return damp;
}

void SolveEqu::solveEquByLLT(const VectorXd_t &b, VectorXd_t &x)
{
    //1.
	const std::size_t n = vL_.rows();
    if ((std::size_t)x.size() != n)
    {
        x.resize(n);
    }

    //2.
    if ((std::size_t)b_.size() != n)
    {
        b_.resize(n);
    }

    //3.
    value_t sum;
	for (std::size_t i = 0; i != n; ++i)
	{
		sum = 0.0;
		for (std::size_t k = 0; k != i; ++k)
		{
			sum += vL_(i,k) * b_(k);
		}

		b_(i) = b(i) - sum;
	}
 
	//4.
	for (std::size_t i = n; i;)
	{
		--i;
		sum = 0.0;
		for (std::size_t k = i + 1; k != n; ++k)
		{
			sum += vL_(k,i) * x(k);
		}

		x(i) = b_(i) / vL_(i,i) - sum;
	}
}

}